
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       fms_ahrs.c
  * @author     baiyang
  * @date       2023-7-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "arming_plane.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>

#include <gcs_mavlink/gcs.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_update_ahrs()
{
    ahrs_view_update();
    fms_update_current_loc();

    mb_arming_update_soft_armed();

    // calculate a scaled roll limit based on current pitch
    fms.roll_limit_cd = fms.aparm.roll_limit_cd;
    fms.pitch_limit_min_cd = fms.aparm.pitch_limit_min_cd;

    bool rotate_limits = true;
#if HAL_QUADPLANE_ENABLED
    if (quadplane.tailsitter.active()) {
        rotate_limits = false;
    }
#endif
    if (rotate_limits) {
        fms.roll_limit_cd *= fms.ahrs->cos_pitch;
        fms.pitch_limit_min_cd *= fabsf(fms.ahrs->cos_roll);
    }

    // updated the summed gyro used for ground steering and
    // auto-takeoff. Dot product of DCM.c with gyro vector gives earth
    // frame yaw rate
    fms.steer_state.locked_course_err += ahrs_get_yaw_rate_earth(fms.ahrs) * fms.FAST_Dt;
    fms.steer_state.locked_course_err = math_wrap_PI(fms.steer_state.locked_course_err);
}

/*
  update current_loc Location
 */
void fms_update_current_loc(void)
{
    fms.have_position = ahrs_get_location(fms.ahrs, &fms.current_loc);

    // re-calculate relative altitude
    fms.relative_altitude = ahrs_get_relative_position_u_home(fms.ahrs) * 0.01f;
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
void fms_ahrs_init()
{
    ahrs_view_init();
}

/*------------------------------------test------------------------------------*/


